#include "over_plan.h"
#include "rt_servo_plan.h"
#include "axisgroup_robot.h"

axisgroup::OverPlan::OverPlan(Robot* grp) : grp_(grp) {}

int axisgroup::OverPlan::active_servo(bool en)
{
    servo_active_ = en;
    if (en)
    {
        rt_plan_servo_.reset();
        rt_plan_servo_ = std::make_unique<axisgroup::RtPlanServo>(0);
        auto moving = grp_->get_moving_state();
        AxisGroupPose pose;
        pose.jpos = moving.cmd.cmd_pos;
        pose.pose = moving.cmd.cmd_pose;
        rt_plan_servo_->init(grp_->kine(), pose, grp_->config_.servo_time_sec);
        rt_plan_servo_->set_active(true);
    }
    return 0;
}